import cv2
import numpy as np
import glob
import copy

def stereo_camera_calibration(chessboard_images_A, chessboard_images_B, chessboard_size=(9, 6)):
    """
    对两个相机进行标定以获得内参，并进行立体标定以获得相机间的旋转矩阵和平移向量。
    :param chessboard_images_A: 相机A的包含棋盘格图像的文件路径列表
    :param chessboard_images_B: 相机B的包含棋盘格图像的文件路径列表
    :param chessboard_size: 棋盘格的尺寸（默认为9x6）
    :return: 两个相机的内参矩阵、畸变系数、旋转矩阵和平移向量
    """
    def calibrate_camera(chessboard_images):
        # 准备物体坐标点
        objp = np.zeros((chessboard_size[1] * chessboard_size[0], 3), np.float32)
        objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)

        # 存储所有图像的物体点和图像点
        objpoints = []  # 真实世界中的点
        imgpoints = []  # 图像中的点

        for fname in chessboard_images:
            img = cv2.imread(fname)
            gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

            # 查找棋盘格角点
            ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
            if ret == True:
                objpoints.append(objp)
                imgpoints.append(corners)

        # 进行相机标定
        img_size = (img.shape[1], img.shape[0])
        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_size, None, None)
        return mtx, dist, objpoints, imgpoints, img_size

    # 分别标定两个相机
    mtx_A, dist_A, objpoints_A, imgpoints_A, img_size = calibrate_camera(chessboard_images_A)
    mtx_B, dist_B, objpoints_B, imgpoints_B, img_size = calibrate_camera(chessboard_images_B)

    # 立体标定
    criteria = cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001
    ret, mtx_A, dist_A, mtx_B, dist_B, R, T, E, F = cv2.stereoCalibrate(
        objpoints_A, imgpoints_A, imgpoints_B, mtx_A, dist_A, mtx_B, dist_B, img_size, criteria=criteria, flags=cv2.CALIB_FIX_INTRINSIC
    )

    return mtx_A, dist_A, mtx_B, dist_B, R, T


# 将旋转向量转换为旋转矩阵，并与平移向量组合成齐次变换矩阵（4x4）
def get_extrinsics(rvecs, tvecs):
    extrinsics = []
    for rvec, tvec in zip(rvecs, tvecs):
        rotation_matrix, _ = cv2.Rodrigues(rvec)
        extrinsic = np.hstack((rotation_matrix, tvec.reshape(3, 1)))
        extrinsic = np.vstack((extrinsic, [0, 0, 0, 1]))  # 添加齐次坐标行
        extrinsics.append(extrinsic)
    return extrinsics


def get_chessboard(image_path, pattern_size):
    """
    获取棋盘格中单个格子的像素尺寸。

    :param image: 包含棋盘格的图像
    :param pattern_size: 棋盘格的模式尺寸，例如 (6, 9)
    :return: 棋盘格中单个格子的平均像素尺寸
    """
    image = cv2.imread(image_path)
 
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)

    if ret:
        chessboard_pixel_width = np.max(corners[:, 0, 0]) - np.min(corners[:, 0, 0])
        chessboard_pixel_height = np.max(corners[:, 0, 1]) - np.min(corners[:, 0, 1])
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        # 计算相邻角点之间的距离
        distances = []
        for i in range(len(corners) - 1):
            dist = np.linalg.norm(corners[i] - corners[i + 1])
            distances.append(dist)

        # 计算平均距离作为格子的像素尺寸
        avg_dist = np.mean(distances)
        return {
            "chessboard_pixel_width": chessboard_pixel_width,
            "chessboard_pixel_height": chessboard_pixel_height,
            "corner": corners2[0].ravel(),
        }
    else:
        return None

def estimate_face_depth(face_real_width, face_pixel_width, 
                        face_real_height, face_pixel_height,
                        chessboard_real_width, chessboard_pixel_width,
                        chessboard_real_height, chessboard_pixel_height,
                        chessboard_to_camera_distance, camera_intrinsics):
    # 获取相机内参中的焦距
    fx = camera_intrinsics[0, 0]
    fy = camera_intrinsics[1, 1]

    # 计算棋盘格每像素代表的实际长度和高度
    chessboard_pixel_s_length = chessboard_real_width / chessboard_pixel_width
    chessboard_pixel_s_height = chessboard_real_height / chessboard_pixel_height

    # 将人脸的像素宽度和高度转换为归一化相机坐标中的宽度和高度
    face_width_in_camera_coords = (face_pixel_width * chessboard_pixel_s_length) 
    face_height_in_camera_coords = (face_pixel_height * chessboard_pixel_s_height) 

    # 使用宽度和高度的平均值来估算深度
    face_to_camera_distance = 0.1 * chessboard_to_camera_distance * (
                              (face_real_width / face_width_in_camera_coords + 
                               face_real_height / face_height_in_camera_coords)) / 2

    return face_to_camera_distance


def detect_faces(image_path):
    # 实例化人脸分类器
    face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')  # 确保XML文件路径正确

    # 读取测试图片
    img = cv2.imread(image_path, cv2.IMREAD_COLOR)
    if img is None:
        print("Image not found or invalid image format")
        return

    # 将原彩色图转换成灰度图
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # 开始在灰度图上检测人脸
    faces = face_cascade.detectMultiScale(gray, scaleFactor=1.3, minNeighbors=4)

    # 遍历人脸检测结果
    for (x, y, w, h) in faces:
        # 在原彩色图上画人脸矩形框
        cv2.rectangle(img, (x, y), (x + w, y + h), (255, 255, 0), 2)

    # 缩小图像
    height, width = img.shape[:2]
    size = (int(width * 0.4), int(height * 0.4))
    resized_img = cv2.resize(img, size, interpolation=cv2.INTER_AREA)

    # 显示处理后的图像
    cv2.namedWindow('faces', cv2.WINDOW_AUTOSIZE)
    cv2.imshow('faces', resized_img)

    # 等待退出键
    cv2.waitKey(0)

    # 销毁显示窗口
    cv2.destroyAllWindows()

    return faces

def project_point_A_to_B(image_point_A, depth, mtx_A, mtx_B, R, T):
    """
    将一个点从相机A图像坐标系投影到相机B图像坐标系。
    :param image_point_A: 相机A图像中的点坐标
    :param depth: 点的深度信息
    :param mtx_A: 相机A的内参矩阵
    :param mtx_B: 相机B的内参矩阵
    :param R: 相机A到相机B的旋转矩阵
    :param T: 相机A到相机B的平移向量
    :return: 投影到相机B图像坐标系中的点
    """
    # 将图像点转换为相机A的相机坐标系中的点
    point_cam_A = np.dot(np.linalg.inv(mtx_A), np.array([image_point_A[0], image_point_A[1], 1])) * depth
    point_cam_A = point_cam_A.reshape(-1, 1)
    # 使用相机A到相机B的转换
    point_cam_B = np.dot(R, point_cam_A) + T.reshape(-1, 1)
    # 将点投影到相机B的图像坐标系
    point_image_B = np.dot(mtx_B, point_cam_B)
    point_image_B = point_image_B / point_image_B[2]  # 归一化


    return point_image_B[:2]

def projection(cordinate, frame):
    cordinate = list(cordinate)
    # print(f"R: {cordinate}")
    cordinate[2] = cordinate[2] - cordinate[0]
    cordinate[3] = cordinate[3] - cordinate[1]
    cordinate = [int(a) for a in cordinate]
    image_chess_path_1 = "./projection/test2/chessborad_A/chess1.jpg"
    image_chess_path_2 = "./projection/test2/chessborad_B/chess2.jpg"
    pattern_size = (6,9)
    chessboard_real_width = 0.018 * 6 # 棋盘格实际尺寸，单位m
    chessboard_real_height = 0.018 * 9
    face_real_width = 0.15  # 人脸实际宽度，单位m
    chessboard_to_camera_distance = 1.05  # 棋盘到相机的实际深度，单位m

    chessboard_pixel_width = get_chessboard(image_chess_path_1, pattern_size)['chessboard_pixel_width']  # 一个棋盘格的宽度
    chessboard_pixel_height  = get_chessboard(image_chess_path_1, pattern_size)['chessboard_pixel_height']  # 一个棋盘格的高度

    # print("Chessboard square size in pixels:", chessboard_pixel_width)
    temp = cordinate
    face_pixel_width = (temp[2] +temp[3])/2  # 人脸在图像中的像素宽度，取宽和高的平均

    chessboard_images_A = glob.glob("./projection/test1/chessborad_A/*.jpg")
    chessboard_images_B = glob.glob("./projection/test1/chessborad_A/*.jpg")
    mtx_A, dist_A, mtx_B, dist_B, R, T = stereo_camera_calibration(chessboard_images_A, chessboard_images_B)

    image_point_A = temp[:2]   # 图像坐标左上角(u, v)

    face_pixel_height = temp[2]
    face_real_height = temp[3]

    # 人脸在图像中的坐标
    chessboard_image_coords = get_chessboard(image_chess_path_1, pattern_size)['corner']  # 棋盘格在图像中的坐标
    depth = estimate_face_depth(face_real_width, face_pixel_width, 
                                    face_real_height, face_pixel_height,
                                    chessboard_real_width, chessboard_pixel_width,
                                    chessboard_real_height, chessboard_pixel_height,
                                    chessboard_to_camera_distance, mtx_A)


    # 调用函数
    point_image_B = project_point_A_to_B(image_point_A, depth, mtx_A, mtx_B, R, T)

    # 读取图片
    image = copy.deepcopy(frame)

    # 定义矩形框的位置和大小
    x,y,w,h = (int(point_image_B[0]), int(point_image_B[1]), temp[2], temp[3])

    # height, width = image.shape[:2] 
    # 绘制矩形框
    print(f"{x} {y} {x+w} {y+h}")
    cv2.rectangle(image,(x,y),(x+w,y+h),(255,255,0),2)
    return [x, y, x+w, y+h], image
    #cv2.rectangle(image, left_top, right_bottom, (255, 255, 0), 2)
    # size = (int(width*0.4), int(height*0.4))  
    # image = cv2.resize(image, size, interpolation=cv2.INTER_AREA)  
